// multi_static_tf_broadcaster.cpp

#include <string>
#include <vector>
#include <sstream>
#include <iostream>
#include <iomanip>
#include <cstring>   // 用于字符串比较
#include <unistd.h>  // 用于 getpid()
#include <sched.h>   // 用于 CPU 绑定

#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2_ros/static_transform_broadcaster.h"
#include "tf2/LinearMath/Quaternion.h"

// 辅助函数：绑定 CPU 核心
bool bind_to_cpu(int cpu_core)
{
    cpu_set_t cpuset;
    CPU_ZERO(&cpuset);
    CPU_SET(cpu_core, &cpuset);
    pid_t pid = getpid();

    if (sched_setaffinity(pid, sizeof(cpu_set_t), &cpuset) == -1)
    {
        std::cerr << "Failed to set CPU affinity to core " << cpu_core << ": " << strerror(errno) << std::endl;
        return false;
    }
    else
    {
        std::cout << "Process is bound to CPU core " << cpu_core << std::endl;
        return true;
    }
}

// 辅助函数：拆分字符串
std::vector<std::string> split_string(const std::string &str, char delimiter)
{
    std::vector<std::string> tokens;
    std::stringstream ss(str);
    std::string item;
    while (std::getline(ss, item, delimiter))
    {
        tokens.push_back(item);
    }
    return tokens;
}

class MultiStaticTransformBroadcaster : public rclcpp::Node
{
public:
    MultiStaticTransformBroadcaster(const std::string &node_name)
        : Node(node_name)
    {
        // 初始化 StaticTransformBroadcaster
        broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);

        // 声明参数 'transforms'，并指定默认值为包含一个空字符串的列表
        this->declare_parameter<std::vector<std::string>>("transforms", std::vector<std::string>{"0 0 0 0 0 0 map odom"});

        // 获取参数
        auto transforms = this->get_parameter("transforms").as_string_array();

        // 解析参数并发布静态变换
        std::vector<geometry_msgs::msg::TransformStamped> static_transforms;
        for (const auto &tf_str : transforms)
        {
            // 期望每个变换为一个字符串，以空格分隔的 8 个值
            // 格式: x y z roll pitch yaw frame_id child_frame_id
            auto parts = split_string(tf_str, ' ');
            if (parts.size() != 8)
            {
                RCLCPP_WARN(this->get_logger(), "Invalid transform format: '%s'. Expected 8 elements.", tf_str.c_str());
                continue;
            }

            try
            {
                double x = std::stod(parts[0]);
                double y = std::stod(parts[1]);
                double z = std::stod(parts[2]);
                double roll = std::stod(parts[3]);
                double pitch = std::stod(parts[4]);
                double yaw = std::stod(parts[5]);
                std::string frame_id = parts[6];
                std::string child_frame_id = parts[7];

                // 创建 TransformStamped 消息
                geometry_msgs::msg::TransformStamped t;
                t.header.stamp = this->get_clock()->now();
                t.header.frame_id = frame_id;
                t.child_frame_id = child_frame_id;
                t.transform.translation.x = x;
                t.transform.translation.y = y;
                t.transform.translation.z = z;

                // 将欧拉角转换为四元数
                tf2::Quaternion quat;
                quat.setRPY(roll, pitch, yaw);
                quat = quat.normalize();

                t.transform.rotation.x = quat.x();
                t.transform.rotation.y = quat.y();
                t.transform.rotation.z = quat.z();
                t.transform.rotation.w = quat.w();

                static_transforms.push_back(t);
                RCLCPP_INFO(this->get_logger(), "Publishing static transform from '%s' to '%s'", frame_id.c_str(), child_frame_id.c_str());
            }
            catch (const std::exception &e)
            {
                RCLCPP_WARN(this->get_logger(), "Error parsing transform '%s': %s", tf_str.c_str(), e.what());
                continue;
            }
        }

        if (!static_transforms.empty())
        {
            // 发布所有静态变换
            broadcaster_->sendTransform(static_transforms);
            RCLCPP_INFO(this->get_logger(), "All static transforms have been published.");
        }
        else
        {
            RCLCPP_WARN(this->get_logger(), "No valid transforms to publish.");
        }
    }

private:
    std::shared_ptr<tf2_ros::StaticTransformBroadcaster> broadcaster_;
};

int main(int argc, char **argv)
{
    // 全局变量替代（类似于 Python 中的 global）
    std::string cpu_core_msg;
    std::string cpu_core_error_msg;

    int cpu_core = -1;
    // 解析命令行参数，查找 --cpu 参数
    for (int i = 1; i < argc; ++i)
    {
        if (std::strcmp(argv[i], "--cpu") == 0 && i + 1 < argc)
        {
            cpu_core = std::atoi(argv[i + 1]);
            // 移除 --cpu 和其值从 argv
            for (int j = i; j + 2 <= argc; ++j)
            {
                argv[j] = argv[j + 2];
            }
            argc -= 2;
            break;
        }
    }

    // 如果指定了 CPU 核心，则尝试绑定
    if (cpu_core != -1)
    {
        if (bind_to_cpu(cpu_core))
        {
            cpu_core_msg = "Process is bound to CPU core " + std::to_string(cpu_core);
            std::cout << cpu_core_msg << std::endl;
        }
        else
        {
            cpu_core_error_msg = "Failed to set CPU affinity to core " + std::to_string(cpu_core);
            std::cerr << cpu_core_error_msg << std::endl;
        }
    }
    else
    {
        cpu_core_error_msg = "No CPU core specified or failed to parse --cpu argument.";
        std::cerr << cpu_core_error_msg << std::endl;
    }

    // 初始化 ROS2
    rclcpp::init(argc, argv);

    // 创建节点
    auto node = std::make_shared<MultiStaticTransformBroadcaster>("multi_static_tf_broadcaster");

    // 如果有 CPU 绑定消息，记录到 ROS2 日志
    if (!cpu_core_msg.empty())
    {
        RCLCPP_INFO(node->get_logger(), "%s", cpu_core_msg.c_str());
    }
    else
    {
        RCLCPP_ERROR(node->get_logger(), "%s", cpu_core_error_msg.c_str());
    }

    // 运行节点
    rclcpp::spin(node);

    // 关闭 ROS2
    rclcpp::shutdown();
    return 0;
}
